#!/usr/bin/python
# -*- coding: UTF-8 -*-

from MiniBotControlLib import MiniBotControlLib
import time
import BwHelpLib

class ArmRobot:

    #初始化机器人：使用 BwRobotLib 对象
    def __init__(self,control_lib):
        self.lib = control_lib
        #ID 列表
        self.ids_size = 0
        self.ids = []

        #当前关节角度
        self.position_angles = []


    #绑定操作臂ID
    def bindIds(self,ids):
        size = len(ids)
        self.ids_size = size
        self.ids = [0] * size
        self.position_angles = [0] * size

        for i in range(size):
            self.ids[i] = ids[i]

    #读取操作臂当前角度位置
    def updateAngles(self, print_flag=False):
        for i in range(self.ids_size):
          #设置位置模式
          self.lib.SetMotorMode(self.ids[i],0)
          #预留时间给设备响应
          time.sleep(0.05)
          #读取第i个关节当前位置
          self.position_angles[i] = self.lib.GetMotorPosition(self.ids[i])
          #预留时间给设备响应
          time.sleep(0.05)
        if print_flag == True:
            print("关节模块ID: ", self.ids)
            print("关节模块角度: ",self.position_angles)
    
    #设置角度位置
    def setJointAngles(self, angles ,speed=20, wait_time=0.3):
        size = len(angles)
        idsLen = len(self.ids)
        speeds = [speed]*idsLen
        #赋值到当前变量
        for i in range(size):
            if(i < idsLen):
                self.position_angles[i] = int(angles[i])

        #将当前变量设给硬件
        self.lib.SetMotorPositionSyncWithSpeeds(self.ids,self.position_angles,speeds, wait_time)   #速度值 36 与 37 单位不一致 36 30% 可能对应 37 2%


    #将角度值推到硬件
    def pushJointsAngles(self, speed=40, wait_time=2):
        self.setJointAngles(self.position_angles,speed,wait_time)


    #夹起
    #I T T I T (I) G
    #0 90 12 0 60 90
    #I T  T  T  I  G
    #0 90 12 60 60 90
    def pickUp(self):
        #恢复初始状态
        self.position_angles = [0] * self.ids_size
        self.pushJointsAngles(speed=20, wait_time=3)

        #下探张开
        self.position_angles = [0, 90, 12, 60, 60, 90]
        self.pushJointsAngles(speed=20, wait_time=3)

        #（下探）夹紧
        self.position_angles = [0, 90, 12, 60, 60, 0]
        self.pushJointsAngles(speed=20, wait_time=3)

        #（下探）->抬起
        self.position_angles[2] = -90
        self.pushJointsAngles(speed=20, wait_time=3)

    #放下
    #I T T I T (I) G
    #0 90 -90 0 60 90
    #90 0
    #       12
    #90 90 30 0 60 90
    def putDown(self):
        #转向
        self.position_angles[0] = 90
        self.pushJointsAngles(speed=20, wait_time=3)

        #下探
        self.position_angles[2] = 12
        self.pushJointsAngles(speed=20, wait_time=3)

        #（下探）->松开
        self.position_angles[self.ids_size - 1] = 90
        self.pushJointsAngles(speed=20, wait_time=3)

        #抬起初始状态
        self.position_angles = [0] * self.ids_size
        self.pushJointsAngles(speed=20, wait_time=3)

    def goHome(self, speeds=5, wait_time=3):
        print("机器人正运动至起始点")
        self.position_angles = [0, 40, 40, 50, 0, 0]
        self.pushJointsAngles(speeds, wait_time)

    # 机器人构型：ITTTITG；
    # I模块的运动范围是（180, -180）
    # T模块的运动范围是（90, -90）
    # G模块的运动范围是（90, -90）
    def keyboardControl(self, cmd):
        # 每次按键，机器人关节运动2度
        joint_step = 2 
        # q键和a键表示第一轴的正转和反转，I模块
        if(cmd == 'q'):
            val = self.position_angles[0] + joint_step
            self.position_angles[0] = BwHelpLib.fixValue(val, 180, -180)
        if(cmd == 'a'):
            val = self.position_angles[0] - joint_step
            self.position_angles[0] = BwHelpLib.fixValue(val, 180, -180)
        # 表示第二轴的正转和反转，T模块
        if(cmd == 'w'):
            val = self.position_angles[1] + joint_step
            self.position_angles[1] = BwHelpLib.fixValue(val, 90, -90)
        if(cmd == 's'):
            val = self.position_angles[1] - joint_step
            self.position_angles[1] = BwHelpLib.fixValue(val, 90, -90)
        # 表示第三轴的正转和反转，T模块
        if(cmd == 'e'):
            val = self.position_angles[2] + joint_step
            self.position_angles[2] = BwHelpLib.fixValue(val, 90, -90)
        if(cmd == 'd'):
            val = self.position_angles[2] - joint_step
            self.position_angles[2] = BwHelpLib.fixValue(val, 90, -90)
        
        # 表示第四轴的正转和反转，T模块
        if(cmd == 'r'):
            val = self.position_angles[3] + joint_step
            self.position_angles[3] = BwHelpLib.fixValue(val, 90, -90)
        if(cmd == 'f'):
            val = self.position_angles[3] - joint_step
            self.position_angles[3] = BwHelpLib.fixValue(val, 90, -90)
        
        # 表示第五轴的正转和反转，I模块
        if(cmd == 't'):
            val = self.position_angles[4] + joint_step
            self.position_ngles[4] = BwHelpLib.fixValue(val, 180, -180)
        if(cmd == 'g'):
            val = self.position_angles[4] - joint_step
            self.position_angles[4] = BwHelpLib.fixValue(val, 180, -180)
        
        # 表示第六轴的正转和反转，G模块
        if(cmd == 'y'):
            val = self.position_angles[5] + joint_step
            self.position_angles[5] = BwHelpLib.fixValue(val, 90, -90)
        if(cmd == 'h'):
            val = self.position_angles[5] - joint_step
            self.position_angles[5] = BwHelpLib.fixValue(val, 90, -90)
        # 打印机器人的当前关节角度
        if(cmd == '1'):
            self.updateAngles(True)
        # 将键盘控制关节角变化的值发送至机器人执行运动
        self.pushJointsAngles(speed=90, wait_time=0.1) 